#!/bin/bash

# EPSILON Autoware Universe Setup Script
# This script sets up the EPSILON workspace following Autoware conventions

set -e

echo "=== EPSILON Autoware Universe Setup ==="

# Check if ROS2 galactic is installed
if ! command -v ros2 &> /dev/null; then
    echo "ROS2 galactic not found. Please install ROS2 galactic first."
    echo "Visit: https://docs.ros.org/en/galactic/Installation.html"
    exit 1
fi

# Source ROS2
source /opt/ros/galactic/setup.bash

# Install dependencies
echo "Installing dependencies..."
sudo apt update
sudo apt install -y \
    python3-colcon-common-extensions \
    python3-colcon-mixin \
    python3-vcstool \
    python3-rosdep \
    ros-galactic-eigen3-cmake-module \
    libeigen3-dev \
    ros-galactic-tf2-geometry-msgs \
    ros-galactic-geometry2 \
    ros-galactic-rviz2 \
    ros-galactic-rviz-common \
    ros-galactic-rviz-default-plugins \
    ros-galactic-rosidl-default-generators \
    ros-galactic-rosidl-default-runtime \
    ros-galactic-ament-cmake-auto \
    ros-galactic-ament-lint-auto \
    ros-galactic-ament-lint-common \
    libboost-all-dev \
    libjsoncpp-dev \
    libopencv-dev \
    python3-numpy \
    python3-matplotlib

# Initialize rosdep if needed
if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
    echo "Initializing rosdep..."
    sudo rosdep init
fi

# Update rosdep
rosdep update

# Install package dependencies
echo "Installing package dependencies..."
rosdep install --from-paths . --ignore-src -r -y

# Build workspace
echo "Building workspace..."
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

# Setup environment
echo "Setting up environment..."
if ! grep -q "source.*autoware_universe.*install.*setup.bash" ~/.bashrc; then
    echo "source $(pwd)/install/setup.bash" >> ~/.bashrc
fi

echo "=== Setup Complete ==="
echo ""
echo "To get started:"
echo "1. Source the workspace: source install/setup.bash"
echo "2. Launch the system: ros2 launch planning_bringup planning.launch.py"
echo ""
echo "For more information, see WORKSPACE_README.md"